# -*- coding:utf-8 -*-
# learn how to import dll and exploit functions within


from ctypes import *
import numpy as np
from cmsg import *
import os 
double36 = c_double*36
double9  = c_double*9
double6  = c_double*6
double3  = c_double*3
dir_path = os.path.dirname(os.path.realpath(__file__))
dllname = dir_path+'\PropagateDLL_0.dll'
LIB_ORBIT_PROP= cdll.LoadLibrary(dllname)

dllname = dir_path+'\PropagateDLL_0.dll'
LIB_ORBIT_PROP_TRUE = cdll.LoadLibrary(dllname)

DEFAULTINITPSE = np.diag((10,10,10,1e-6,1e-6,1e-6))
LIGHTSPEED     = np.inf  # 299792458
DEFAULTMEARSUREMENTVAR = 1e6 # estimatied variance of ranging data 
MEARSUREMENTNOISEVAR = 0  # to generate noise of measurement
BACK_LINK_DELAY  = 10 # after this seconds will the second link be established

class cStateWrapper(normalDistributeMsg):
    '''
        a state wrapper describle a n-dim normal distributed random variable
    '''
    def __init__(self,mean=None,cov=None):
        if mean is None:
            mean=np.zeros((1))
        if cov is None:
            cov=np.zeros((1))
        normalDistributeMsg.__init__(self,'wrapper',mean,cov)

    def changeForm (self,driftMean=None,driftCov=None):
        '''
            change the state's form to match the dll 's need, ctypes
            driftMean: add Drift state to the output mean
            TODO why add driftMean and driftState?
        '''
        meanFormed = []
        covFormed  = []
        assert(len(self.mean) in [3,6])
        assert(self.cov.size == len(self.mean)**2)
        if(driftMean is None)==False:
            assert(isinstance(driftMean,np.ndarray))
        if(len(self.mean) ==3):
            if ( driftMean is None )==False:
                meanFormed = double6(6)
                covFormed  = double36(0)
            else:
                meanFormed = double3(0)
                covFormed  = double9(0)
            for i in range(0,3):
                meanFormed[i] = self.mean[i]
            if ( driftMean is None )==False:
                for i in range(3,6):
                    meanFormed[i] =  driftMean[i-3]
                #cov
                if (driftCov is None )==False:
                    for  i  in range(3,6):
                        for j  in range(3,6):
                            covFormed[i*6+j] = driftCov[i-3,j-3]
            for i in range(0,9):
                covFormed[i]  = self.cov[i/3,i%3]
        else:  # 6
            meanFormed = double6(0)
            covFormed  = double36(0)
            for i in range(0,6):
                meanFormed[i] = self.mean[i]
            for i in range(0,36):
                covFormed[i]  = self.cov[i/6,i%6]
        return [meanFormed,covFormed]
    def partState(self,start=0,end=None):
        if(end  is None):
            end = self.mean.size-1
        mean = np.zeros((end+1-start))
        cov  = np.zeros((end+1-start,end+1-start))
        for i in range(start,end+1):
            try:
                mean[i] = self.mean[i]
            except:
                disp('error')            
            for j in range(start,end+1):
                cov[i,j] = self.cov[i,j]
        return(cStateWrapper(mean,cov))
    def __sub__(self,other):
        assert(isinstance(other,normalDistributeMsg))
        assert(self.dim == other.dim)
        return cStateWrapper(self.mean-other.mean,self.cov+other.cov)

    
def createStateWrapperFromPropOutPut(output):
    '''
        output is IntergOutPut
    '''
    x= []
    xcov = []
    for i in range(0,6):
        x.append(output.X[i])
    for i in range(0,36):
        xcov.append(output.dPse[i])
    return (cStateWrapper(np.array(x,dtype=float),np.array(xcov).reshape(6,6)))

def createStateWrapperFromMsg(msg):
    '''
        This Message should be a normalDistributeMsg
    '''
    assert(isinstance(msg,normalDistributeMsg))
    return(cStateWrapper(msg.mean,msg.cov))


class UTCtime(Structure):
    '''
        int Year;
        int Mon;
        int Day;
        int Hour;
        int Min;
        
        double Sec;
        double JD;
        double IntJD;	  /*JD的小数部分*/
        double DouJD;    /*JD的整数部分*/
    '''
    _fields_  = [("Year",c_int),
                ("Mon",c_int),
                ("Day",c_int),
                ("Hour",c_int),
                ("Min",c_int),
                ("Sec",c_double),
                ("JD",c_double),
                ("IntJD",c_double),
                ("DouJD",c_double)]

class BDT(Structure):
    '''
            struct BDTime
        {
            int weekNum;
            double SOW;

        };
    '''
    _fields_  = [("weekNum", c_int),
                 ("SOW", c_double)
    ]
def time2BDT(floatTime):
    week =  floatTime/604800
    sow  =  floatTime % 604800
    return BDT(int(week),sow)
def JD2BDT(JDTime):
    week   = c_int()
    sow    = c_double()
    libOrbitProp.NavTime(JDTime[0],JDTime[1],JDTime[2],JDTime[3],JDTime[4],c_double(JDTime[5]),byref(week),byref(sow))
    return BDT(int(week.value),sow.value)
def JD2Time(JDTime):
    week   = c_int()
    sow    = c_double()
    libOrbitProp.NavTime(JDTime[0],JDTime[1],JDTime[2],JDTime[3],JDTime[4],c_double(JDTime[5]),byref(week),byref(sow))
    return int(week.value)*604800+ sow.value
class IntergInPut(Structure):
    '''
        BDT startTime;//初始时间
        BDT endTime;//初始时间
        double X[6];                /*初始状态*/
        double dPse[36];                /*系统协方差矩阵元素*/
        double dQeph[36];              /*系统过程协方差矩阵元素*/
        double dPsc[9];                /*系统协方差矩阵元素*/
        double dQclk[9];  

        double dAsat;			   	      /*卫星面质比参数*/
        double dCrad1;   /*光压系数*/
    '''
    _fields_  = [("startTime",BDT),
            ("endTime",BDT),
            ("X",double6),
            ("dPse",double36),
            ("dQeph",double36),
            ("dPsc",double9),
            ("dQclk",double9),
            ("dAsat",c_double),
            ("dCrad1",c_double)]
class IntergOutPut(Structure):
    '''
    	double X[6];                /*结束状态*/
        double dPse[36];                /*系统协方差矩阵元素*/
        double dPsc[9]; 
    '''
    _fields_  = [("X",double6),
                 ("dPse",double36),
                 ("dPsc",double36)]

def constructIntergInput(inputData1StartTime,
                         inputData1EndTime,
                         inputData1X,
                         PMatrixPara_dPse=None,
                         QMatrixPara_dQeph=None,
                         PMatrixPara_dPsc=None,
                         QMatrixPara_dQclk=None):

    inputData1dAsat = 0.002
    inputData1dCrad1 = 1.2
    startTime  = inputData1StartTime.weekNum *604800+ inputData1StartTime.SOW
    endTime  = inputData1EndTime.weekNum *604800+ inputData1EndTime.SOW
    timePeriod = abs(startTime-endTime)
    times      = (timePeriod/300)
    if(PMatrixPara_dPse is None):
        PMatrixPara_dPse   = double36(0)
            # /*默认噪声及启动参数*/
        PMatrixPara_dPse[0 * 6 + 0] = 100
        PMatrixPara_dPse[1 * 6 + 1] = 100
        PMatrixPara_dPse[2 * 6 + 2] = 100
        PMatrixPara_dPse[3 * 6 + 3] = 1e-6
        PMatrixPara_dPse[4 * 6 + 4] = 1e-6
        PMatrixPara_dPse[5 * 6 + 5] = 1e-6
    if(QMatrixPara_dQeph is None):
        QMatrixPara_dQeph  = double36(0)
        QMatrixPara_dQeph[0] = 2e-7 *times
        QMatrixPara_dQeph[1 * 6 + 1] = 2e-7*times
        QMatrixPara_dQeph[2 * 6 + 2] = 2e-7*times
        QMatrixPara_dQeph[3 * 6 + 3] = 2e-10*times
        QMatrixPara_dQeph[4 * 6 + 4] = 2e-10*times
        QMatrixPara_dQeph[5 * 6 + 5] = 2e-10*times
    if(PMatrixPara_dPsc is None):
        PMatrixPara_dPsc   = double9(0)
        PMatrixPara_dPsc[0 * 3 + 0] = 1e-20
        PMatrixPara_dPsc[1 * 3 + 1] = 1e-30
        PMatrixPara_dPsc[2 * 3 + 2] = 1e-40
    if(QMatrixPara_dQclk is None):
        QMatrixPara_dQclk  = double9(0)
        QMatrixPara_dQclk[0 * 3 + 0] = 1.998002997000202e-019
        QMatrixPara_dQclk[1 * 3 + 1] = 9.990001493999999e-030
        QMatrixPara_dQclk[2 * 3 + 2] = 4.980000000000000e-041
    return IntergInPut(inputData1StartTime,
                        inputData1EndTime,
                        inputData1X,
                        PMatrixPara_dPse,
                        QMatrixPara_dQeph,#
                        PMatrixPara_dPsc,
                        QMatrixPara_dQclk,#
                        inputData1dAsat,
                        inputData1dCrad1)

if __name__ == '__main__':
    # import time
    # inputData1StartTime = BDT(462,518400-1.0) # This is a question
    # inputData1EndTime   = BDT(462,518401-1.0)
    # inputData1X         = double6(-36031725.2928148,
    #                             21816922.7212969,
    #                             0.0000000,
    #                             -914.3388243,
    #                             -1510.0757225,
    #                             2521.1339803)

    # inputData1  =  constructIntergInput(inputData1StartTime,
    #                                     inputData1EndTime,
    #                                     inputData1X
    #                                     )
    # outputData1 = IntergOutPut()

    # # pinputData1 = pointer(inputData1)
    # # poutputData1= pointer(outputData1)
    # startTime   =  time.time()
    # libOrbitProp.Integrator_Module(byref(inputData1),byref(outputData1))
    # endTime     =  time.time()
    # print 'the time total time is %0.10f'%(endTime-startTime)


    initiaTime = 307627203.0
    initialBDT = time2BDT(initiaTime)
    inputData1X = double6(16983361.424781 , 
                          -20353706.441348 ,
                         -0.001558  ,
                          1709.450049 ,
                           1426.384334  ,
                           3179.607804)
    inputData2X = double6(-3766833.202231  ,
                          21353110.556315 ,
                           15407042.875542    ,
                            -2957.880019     ,
                            1099.463044  ,
                          -2237.488044)
    link1EstablishTime = 508* 604800+388804.50
    sat1emitTime       = link1EstablishTime- 49065561.990576 /LIGHTSPEED
    sat1emit      = IntergOutPut()
    inputData1      =  constructIntergInput( initialBDT,time2BDT(sat1emitTime),inputData1X)
    libOrbitProp.Integrator_Module(byref(inputData1),byref(sat1emit))
    sat1emitState  = createStateWrapperFromPropOutPut(sat1emit)
    print 'sat1 emit position:'+sat1emitState.__str__()
    print 'link1EstablishTime: %f'%link1EstablishTime
    print 'sat1emitTime: %f'%sat1emitTime


    sat2rec  =   IntergOutPut()
    inputData1     = constructIntergInput( initialBDT,time2BDT(link1EstablishTime),inputData2X)
    libOrbitProp.Integrator_Module(byref(inputData1),byref(sat2rec))
    sat2recState  = createStateWrapperFromPropOutPut(sat2rec)
    print 'sat2 REC position:'+sat2recState.__str__()


    link2EstablishTime  =  508* 604800+ 388806.00
    sat2emitTime       = link2EstablishTime- 49065067.955931/LIGHTSPEED
    sat2emit  =   IntergOutPut()
    inputData3X = sat2recState.changeForm()

    inputData1     = constructIntergInput( time2BDT(link1EstablishTime),time2BDT(sat2emitTime),inputData3X[0],inputData3X[1])
    libOrbitProp.Integrator_Module(byref(inputData1),byref(sat2emit))
    sat2emitState  = createStateWrapperFromPropOutPut(sat2emit)
    print 'link2EstablishTime: %f'%link2EstablishTime
    print 'sat2 emit position:'+sat2emitState.__str__()
    print 'sat2emitTime: %f'%sat2emitTime

    sat1rec  =   IntergOutPut()
    inputData4X = sat1emitState.changeForm()

    inputData1     = constructIntergInput( time2BDT(sat1emitTime),time2BDT(link2EstablishTime),inputData4X[0],inputData4X[1])
    libOrbitProp.Integrator_Module(byref(inputData1),byref(sat1rec))
    sat1recState  = createStateWrapperFromPropOutPut(sat1rec)
    print 'sat1 rec position:'+sat1recState.__str__()

    print 'link 1 expected range:%f'%(np.linalg.norm(sat1emitState.mean[0:3]-sat2recState.mean[0:3]))
    print 'link 1 pesudorange: 49065561.990576'
    print 'link 2 expected range:%f'%(np.linalg.norm(sat2emitState.mean[0:3]-sat1recState.mean[0:3]))
    print 'link 2 pesudorange: 49065067.955931'    



    
    # measurement: 0   508 388804.50 01 04   49065561.990576         -5.061735
    # 0   508 388806.00 04 01   49065067.955931         -5.062511